import java.io.DataOutputStream;
import java.io.IOException;
import java.io.OutputStream;

import java.util.ArrayList;
import java.util.Queue;


import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.EOPD;
import lejos.nxt.addon.IRLink;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.NXTConnection;
import lejos.robotics.navigation.TachoPilot;


public class Labyrinth {
	static Motor rightMotor    	 	 = Motor.A;
	static Motor leftMotor     	 	 = Motor.B;
	static EOPD  rightIRSensor 		 = new EOPD(SensorPort.S2);
	static EOPD  leftIRSensor  		 = new EOPD(SensorPort.S3);
	static UltrasonicSensor usSensor = new UltrasonicSensor(SensorPort.S1);
	static TouchSensor tSensor 		 = new TouchSensor(SensorPort.S4);	
	static Queue messageQueue		 = new Queue();
	static ArrayList<Node> nodeList	 = new ArrayList<Node>();
	static int x = 0;
	static int y = 0;
	static boolean connected = false;
	static int signalStrength = 0;
	static int angle=0;
	
	static volatile int usSensorValue = 4;
	static volatile int leftIRValue   = 0;
	static volatile int rightIRValue  = 0;

	private static final int DEFAULT_WALL_DISTANCE = 830;
	private static final int IR_INFINITY_DISTANCE  = 990;
	private static final float CORRECTION_FACTOR   = 1.5f;
	private static final  int IR_BUFFER_VALUE 	   = 30;
	private static TachoPilot tp = new TachoPilot(56, 11.5f, leftMotor, rightMotor);
	private static Ausrichter ausrichter;

	static int turnCounter = 0;


	static final int defaultMotorSpeed = 160;
	
	static ArrayList<Thread> threads = new ArrayList<Thread>();
	
	public static void main(String[] args) throws InterruptedException {
		leftIRSensor.setModeLong();
		rightIRSensor.setModeLong();
		startThreads();
	}

	private static void startThreads() {
		Thread displayOutputThread = new Thread() {
			@Override
			public void run() {
				try {
					displayOutput();
				} catch (Exception e) {}
			}
		};
		displayOutputThread.start();
		threads.add(displayOutputThread);
		
		Thread driveThread = new Thread() {
			@Override
			public void run() {
				try {
//					pledge();
//					map();
					pledgeMap();
				} catch (Exception e) {}
			}
		};
		driveThread.start();
		threads.add(driveThread);
		
		Thread stopThread = new Thread() {
			@Override
			public void run() {
				try {
					stop();
				} catch (Exception e) {}
			}
		};
		stopThread.start();
		threads.add(stopThread);
		
		Thread bluetoothThread = new Thread() {
			@Override
			public void run() {
				try {
					initComm();
				} catch (Exception e) {}
			};
		};
		bluetoothThread.start();
		threads.add(bluetoothThread);
		
		ausrichter = new Ausrichter(leftIRSensor,rightIRSensor,leftMotor,rightMotor);
		ausrichter.start();
		stopThread.setPriority(Thread.MAX_PRIORITY);
		driveThread.setPriority(Thread.MAX_PRIORITY);
	}
	
	private static void initComm() throws IOException, InterruptedException {
	    while(true) {
	    	line1="waiting for connection";  
	    

		    BTConnection btc = Bluetooth.waitForConnection();
		    line1="connection established";
		    connected = true;
		
		    OutputStream os = btc.openOutputStream();
		    DataOutputStream dos = new DataOutputStream(os);
		
		    while(true) {
		    	signalStrength = btc.getSignalStrength();
		    	if(signalStrength == 0) {
		    		break;
		    	}
		    	if(!messageQueue.empty()) {
		    		dos.writeChars((String)messageQueue.pop()+'\n');
			    	dos.flush();
		    	} else {
			    	Thread.sleep(200);
		    	}
		    }
	    	Thread.sleep(200);
	    }
	    
	}

	private static void stop() throws InterruptedException {
		while(true) {
			if(tSensor.isPressed()) {
				System.exit(0);
			}
			
			usSensorValue = usSensor.getDistance();
			leftIRValue   = leftIRSensor.readRawValue();
			rightIRValue  = rightIRSensor.readRawValue();
			Thread.sleep(100);
		}
	}
	private static String line1 ="";
	
	private static void displayOutput() throws InterruptedException {
		while (true) {
			LCD.clear();
			LCD.drawString(line1, 0,0);
			LCD.drawString("Signal: " + String.valueOf(signalStrength), 0, 1);
			LCD.drawString("IR rechts: " + rightIRValue, 0, 2);
			LCD.drawString("IR links:  " + leftIRValue, 0, 3);
			LCD.drawString("Distance:  "   	 + tp.getTravelDistance(), 0, 4);
			LCD.drawString("US: " + usSensorValue, 0, 5);
			LCD.drawString("x: " + x +", y: "+ y, 0, 6);

			int breite = (int) (ausrichter.getAbstand() * 10);
			int lastb  = (int) (ausrichter.getBreite() * 10);
			int bd     = (int) (ausrichter.getWinkel() * 10);
			LCD.drawString(breite + "; " + lastb + "; " + bd, 0,7);
			
			Thread.sleep(250);
		}
	} 
	
	private static void turn(int degrees) throws InterruptedException {		
		leftMotor.setSpeed(defaultMotorSpeed);
		rightMotor.setSpeed(defaultMotorSpeed);
		
		if(degrees > 0) {
			rightMotor.backward();
			leftMotor.forward();
		} else if(degrees < 0) {
			rightMotor.forward();
			leftMotor.backward();
		}
		
		int lT = leftMotor.getTachoCount();
		int rT = rightMotor.getTachoCount();
		while(true) {
			if(Math.abs(rightMotor.getTachoCount()-rT)+
			   Math.abs(leftMotor.getTachoCount()-lT)>=11/3f*Math.abs(degrees))  
				break;
			Thread.sleep(50);
		}
		
		rightMotor.forward();	
		leftMotor.forward();
	}
	
	private static void pledge() throws InterruptedException {
		tp.setSpeed(defaultMotorSpeed);
		tp.forward();
		
		Thread.sleep(250);
		while(true) {
			int rightIRWert  = rightIRSensor.readRawValue();
			int leftIRWert   = leftIRSensor.readRawValue();
			
			correctLane(leftIRWert, rightIRWert);
			if(usSensorValue>25 && rightIRValue>IR_INFINITY_DISTANCE && leftIRValue>IR_INFINITY_DISTANCE) {
				Sound.beep();
				System.exit(0);
			}
			if(usSensorValue<14 && usSensorValue!=4) {
				if(rightIRWert > IR_INFINITY_DISTANCE) {
					turn(90);
					turnCounter--;
				} else if(leftIRWert > IR_INFINITY_DISTANCE) {
					turn(-90);
					turnCounter++;
				} else {
					turn(180);
					turnCounter-=2;
				}
			} else if(rightIRWert > IR_INFINITY_DISTANCE && turnCounter > 0) {
				turnWithDelay(90,false);
				turnCounter--;
			} else if(leftIRWert > IR_INFINITY_DISTANCE && turnCounter < 0) {
				turnWithDelay(-90,false);
				turnCounter++;
			}
			Thread.sleep(50);
		}
	}
	
	private static void insertNode(int x, int y, boolean wallAhead, boolean wallLeft, boolean wallRight, int angle) {
		for(Node n : nodeList) {
			if (n.x == x && n.y == y) {
				n.editNode(wallAhead, wallLeft, wallRight, angle);
				messageQueue.addElement("#"+x+"|"+y+"|"+wallAhead+"|"+wallLeft+"|"+wallRight+"|"+angle+"\n");
				return;
			}
		}
		messageQueue.addElement("#"+x+"|"+y+"|"+wallAhead+"|"+wallLeft+"|"+wallRight+"|"+angle+"\n");
		nodeList.add(new Node(x,y,wallAhead,wallLeft,wallRight,angle));
	}

	private static void pledgeMap() throws InterruptedException {

		while(!connected) {
			Thread.sleep(5);
		}
		
		tp.setSpeed(defaultMotorSpeed);
		tp.forward();
		
		boolean wallAhead = (usSensorValue<=25&&usSensorValue!=4);
		boolean wallLeft   = leftIRValue < IR_INFINITY_DISTANCE;
		boolean wallRight  = rightIRValue < IR_INFINITY_DISTANCE;
		
		if(!wallAhead) {
			insertNode(x, y, wallAhead, wallLeft, wallRight, angle);
		}
		
		while(true) {			
			wallAhead = (usSensorValue <= 25 && usSensorValue != 4);
			wallLeft   = leftIRValue  < IR_INFINITY_DISTANCE;
			wallRight  = rightIRValue < IR_INFINITY_DISTANCE;

			correctLane(leftIRValue, rightIRValue);
			int tmp = 0;
			if(tp.getTravelDistance()>250-tmp && !wallAhead) {
				switch(angle) {
				case 0: 
					x++; 
				break;
				case 90: 
					y++; 
				break;
				case 180: 
					x--; 
				break;
				case 270: 
					y--; 
				break;
				}
				tmp = (int)tp.getTravelDistance()-250;
				tp.reset();
				
				insertNode(x, y, wallAhead, wallLeft, wallRight, angle);
			}
			if(usSensorValue==255 && !wallLeft && !wallRight) {
				Sound.setVolume(50);
				for (int i=1; i<10; ++i) {
					Sound.beep();
					Thread.sleep(50);
				}
				System.exit(0);
			}
			if(wallAhead && usSensorValue<14) {
				if(!wallRight) {
					turnMessage(true,wallLeft,wallRight,angle);
					turn(90);
					tp.reset();
					angle+=90;
					turnCounter--;
				} else if(!wallLeft) {
					turnMessage(true,wallLeft,wallRight,angle);
					turn(-90);
					tp.reset();
					angle-=90;
					turnCounter++;
				} else {
					turnMessage(true,wallLeft,wallRight,angle);
					turn(180);
					tp.reset();
					angle+=180;
					turnCounter-=2;
				}
				Sound.beep();
			} else if(!wallRight && turnCounter > 0) {
				turnWithDelay(90,true);
				angle += 90;
				turnCounter--;
			} else if(!wallLeft && turnCounter < 0) {
				turnWithDelay(-90,true);
				angle -= 90;			
				turnCounter++;
			}

			if(angle<0) {
				angle=360+angle;
			}

			angle%=360;
			Thread.sleep(1);

		}
	}

	
	
	private static void map() throws InterruptedException {
		int angle = 0;

		while(!connected) {
			Thread.sleep(5);
		}
		
		tp.setSpeed(defaultMotorSpeed);
		tp.forward();
		
		boolean wallAhead = (usSensorValue<=25&&usSensorValue!=4);
		boolean wallLeft   = leftIRValue < IR_INFINITY_DISTANCE;
		boolean wallRight  = rightIRValue < IR_INFINITY_DISTANCE;
		
		if(!wallAhead) {
			insertNode(x, y, wallAhead, wallLeft, wallRight, angle);
		}
		
		while(true) {			
			wallAhead = (usSensorValue < 25 && usSensorValue != 4);
			wallLeft   = leftIRValue  < IR_INFINITY_DISTANCE;
			wallRight  = rightIRValue < IR_INFINITY_DISTANCE;

			correctLane(leftIRValue, rightIRValue);
			int tmp = 0;
			if(tp.getTravelDistance()>250-tmp && !wallAhead) {
				switch(angle) {
				case 0: 
					x++; 
				break;
				case 90: 
					y++; 
				break;
				case 180: 
					x--; 
				break;
				case 270: 
					y--; 
				break;
				}
				tmp = (int)tp.getTravelDistance()-250;
				tp.reset();
				
				insertNode(x, y, wallAhead, wallLeft, wallRight, angle);
			}
			if(!wallAhead && !wallLeft && !wallRight) {
				leftMotor.stop();
				rightMotor.stop();
				Sound.setVolume(50);
				for (int i=1; i<10; ++i) {
					Sound.beep();
					Thread.sleep(50);
				}
				System.exit(0);
			}
			if(wallAhead && usSensorValue < 14) {
				if(!wallRight) {
					turnMessage(true,wallLeft,wallRight,angle);
					turn(90);
					tp.reset();
					
					angle+=90;
				} else if(!wallLeft) {
					turnMessage(true,wallLeft,wallRight,angle);
					turn(-90);
					tp.reset();
					
					angle-=90;
				} else {
					
					turnMessage(true,wallLeft,wallRight,angle);
					turn(180);
					tp.reset();
					
					angle+=180;
				}
			} 
			if(angle<0) {
				angle=360+angle;
			}

			angle%=360;
			Thread.sleep(1);

		}
	}
	
	private static void turnMessage(boolean wallAhead,boolean wallLeft, boolean wallRight, int angle) {
		switch(angle) {
		case 0: 
			x++; 
		break;
		case 90: 
			y++; 
		break;
		case 180: 
			x--; 
		break;
		case 270: 
			y--; 
		break;
		}
		messageQueue.addElement("#"+x+"|"+y+"|"+wallAhead+"|"+wallLeft+"|"+wallRight+"|"+angle+"\n");
		tp.reset();
	}

	private static void turnWithDelay(int degrees,boolean withDealy) throws InterruptedException {
		delay();
		if(withDealy) {
			turnMessage(usSensorValue<=25&&usSensorValue!=4, leftIRValue<IR_INFINITY_DISTANCE, rightIRValue<IR_INFINITY_DISTANCE, angle);
		}
		turn(degrees);
		tp.reset();
		delay();
	}
	
	private static void turnWithDelayAndMessage(int degrees, int angle, boolean wallLeft, boolean wallRight) throws InterruptedException {
		boolean wallAhead = (usSensorValue<=25&&usSensorValue!=4);
		turnMessage(wallAhead, wallLeft, wallRight, angle);
		turnWithDelay(degrees, true);
	}
	
	private static void delay() throws InterruptedException{
		leftMotor.setSpeed(defaultMotorSpeed);
		rightMotor.setSpeed(defaultMotorSpeed);
		int startTachoCount = leftMotor.getTachoCount();
		while (leftMotor.getTachoCount() < startTachoCount+285) {
			int rightIRWert  = rightIRSensor.readRawValue();
			int leftIRWert   = leftIRSensor.readRawValue();
			
			correctLane(leftIRWert, rightIRWert);
			
			Thread.sleep(5);
		}
	}
	
	private static void correctLane(int leftIRWert, int rightIRWert) throws InterruptedException {
		if(leftIRWert > IR_INFINITY_DISTANCE && rightIRWert < DEFAULT_WALL_DISTANCE+IR_BUFFER_VALUE) {
			moveLeft();
		} else if(rightIRWert > IR_INFINITY_DISTANCE && leftIRWert < DEFAULT_WALL_DISTANCE+IR_BUFFER_VALUE) {
			moveRight();
		} else if((rightIRWert<DEFAULT_WALL_DISTANCE-IR_BUFFER_VALUE) || (leftIRWert>DEFAULT_WALL_DISTANCE+IR_BUFFER_VALUE && leftIRWert < IR_INFINITY_DISTANCE)) {
			moveLeft();
		} else if((leftIRWert < DEFAULT_WALL_DISTANCE-IR_BUFFER_VALUE) || (rightIRWert>DEFAULT_WALL_DISTANCE+IR_BUFFER_VALUE && rightIRWert < IR_INFINITY_DISTANCE)) {
			moveRight();
		} else {
			leftMotor.setSpeed(defaultMotorSpeed);
			rightMotor.setSpeed(defaultMotorSpeed);
			tp.setSpeed(defaultMotorSpeed);
		}
	}
	
//	private static int mcl_cnt = 0; 
//	private static void myCorrectLane() {
//		Ausrichter a = null;
//		
//		// double abstandVonMitte = a.getBreite()-a.getAbstand()-(Ausrichter.robotorBreiteCm/2.0);
//		if (mcl_cnt == 10) {
//			tp.rotate(5);
//		}
//		
//		++mcl_cnt;
//	}

	private static void moveLeft() throws InterruptedException {
		leftMotor.setSpeed(defaultMotorSpeed);
		if(rightIRValue< DEFAULT_WALL_DISTANCE - 90) {
			rightMotor.setSpeed((int)(defaultMotorSpeed*2f));
		} else {
		rightMotor.setSpeed((int)(defaultMotorSpeed*CORRECTION_FACTOR));
		}
	}
	private static void moveRight() throws InterruptedException {
		rightMotor.setSpeed(defaultMotorSpeed);
		if(leftIRValue < DEFAULT_WALL_DISTANCE -90) {
			leftMotor.setSpeed((int)(defaultMotorSpeed*2f));
		} else {
			leftMotor.setSpeed((int)(defaultMotorSpeed*CORRECTION_FACTOR));
		}
	}
}